/***************************************************************************************************************************
 * px4_pose_estimator.cpp
 *
 * Author: Wr
 *
 * Update Time: 2021.3.25
 * 
 *
 * Fuction:  px4位置估计
 *      1.  订阅Vins发布的里程计数据作为飞控定位数据来源
 *      2.  Todo 订阅msckf_vio发布的里程计数据作为飞控定位数据来源
 *      3. 从state_from_mavros获取飞控状态信息
 *      4. 发布无人机里程计和轨迹，用于实验分析及显示
 *      5. 发布无人机状态数据给控制程序 px4_control.cpp或其他上层应用程序
 *
***************************************************************************************************************************/

//头文件
#include <ros/ros.h>
#include <iostream>
//#include <Eigen/Eigen>

#include <vector>

//当前功能包include文件夹下工具
#include "state_from_mavros.h"
#include "math_utils.h"


//msg头文件
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/PoseStamped.h>

//rviz可视化专用
#include <visualization_msgs/Marker.h>

#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>

#include <sensor_msgs/Imu.h>

//自定义消息类型
#include  "px4_msgs/Px4State.h"

#define input_source 1      //定位数据来源 1:Vins   2:msckf_vio
#define tra_windows 1000  //轨迹长度

#define camera_pose_bias_x 0.015
#define camera_pose_bias_y 0.048
#define camera_pose_bias_z -0.026

const Eigen::Quaterniond q_body_camera = Eigen::Quaterniond(-0.023, -0.022, -0.005, 0.999).normalized();

using namespace std;

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>函数声明<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
//用于将定位数据发送给飞控
void send_to_px4();
//用于发送无人机当前信息给其他节点使用，显示，如控制节点等
void pub_to_nodes(px4_msgs::Px4State State_from_mavros);
// 发送相机位姿至fast-planner
void send_to_planner();

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>变量声明<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

//---------------------------------------Vins------------------------------------------
//odometry
Eigen::Vector3d px4_pose_vins;
Eigen::Quaterniond px4_q_vins;
Eigen::Vector3d px4_euler_vins;
//camera_pose odometry
Eigen::Vector3d px4_camera_pose_vins;
Eigen::Quaterniond px4_camera_q_vins;
Eigen::Vector3d px4_camera_euler_vins;
//---------------------------------------msckf_vio---------------------------------
Eigen::Vector3d px4_pose_msckf;
Eigen::Quaterniond px4_q_msckf;
Eigen::Vector3d px4_euler_msckf;


//---------------------------------------无人机状态Px4State---------------------------------
px4_msgs::Px4State Px4_State;   //从state_from_macros的公有变量px4_state中获取飞控当前状态

//px4轨迹数组
std::vector<geometry_msgs::PoseStamped> px4_pose_history;



//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>定义发布者<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
//vins相关
ros::Publisher vins_pose_pub;
ros::Publisher camera_pose_pub;

//msckf相关

//发布给飞控定位
ros::Publisher vision_mavros_pub;
ros::Publisher px4_state_pub;

//导航、rviz显示相关
//发布px4的odometry(这里是从飞控读取的实际值)，用于导航及rviz显示
ros::Publisher px4_odom_pub;
//发布px4的轨迹
ros::Publisher px4_trajectory_pub;
//发布Marker
ros::Publisher px4_marker_pub;

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回调函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

void vins_cb(const nav_msgs::Odometry::ConstPtr &msg)
{
    px4_pose_vins = Eigen::Vector3d(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z);
    px4_q_vins = Eigen::Quaterniond(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z);
    px4_euler_vins = quaternion_to_euler(px4_q_vins);
}
//回调，获取vins估计的camera_pose
void vins_camera_cb(const nav_msgs::Odometry::ConstPtr &msg)
{
    px4_camera_pose_vins = Eigen::Vector3d(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z);
    px4_camera_q_vins = Eigen::Quaterniond(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z);
    px4_camera_euler_vins = quaternion_to_euler(px4_camera_q_vins);
}

void msckf_cb(const nav_msgs::Odometry::ConstPtr &msg)
{
    px4_pose_msckf = Eigen::Vector3d(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z);
    px4_q_msckf = Eigen::Quaterniond(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z);
    px4_euler_msckf = quaternion_to_euler(px4_q_vins);
}



void timerCallback(const ros::TimerEvent &e)
{
    ROS_INFO("Program is running.");
}

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
    ros::init(argc, argv, "px4_pose_estimator");
    ros::NodeHandle nh("~");
   
    // 【订阅】Vins估计的里程计数据    因为Vins出来的话题为里程计数据，这里直接订阅后做一个消息类型的转换即可
    ros::Subscriber vins_sub = nh.subscribe<nav_msgs::Odometry>("/vins_estimator/odometry", 100, vins_cb);
    // 【订阅】Vins估计的相机里程计数据 ，方便后面建图使用
    ros::Subscriber vins_camera_sub = nh.subscribe<nav_msgs::Odometry>("/vins_estimator/camera_pose", 100, vins_camera_cb);

    // 【订阅】Msckf_vio估计位姿
    ros::Subscriber msckf_sub = nh.subscribe<nav_msgs::Odometry>("/firefly_sbx/vio/odom", 100, msckf_cb);

    // 【发布】发布定位数据给飞控
    vision_mavros_pub = nh.advertise<geometry_msgs::PoseStamped>("/mavros/vision_pose/pose", 100);
    //  【发布】发布无人机当前状态给其他节点
    px4_state_pub = nh.advertise<px4_msgs::Px4State>("/px4/state", 10);

    // 【发布】vins估计的相机姿态给fastplanner
    camera_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/px4/vins/camera_pose", 100);
    // 【发布】从mavros读取的px4当前的里程计，用于分析与显示
    px4_odom_pub = nh.advertise<nav_msgs::Odometry>("/px4/odom", 10);
    // 【发布】px4运动轨迹，用于分析，显示
    px4_trajectory_pub = nh.advertise<nav_msgs::Path>("/px4/trajectory", 10);
    // 【发布】px4 Marker点，用于rviz显示
    px4_marker_pub = nh.advertise<visualization_msgs::Marker>("/px4/state/marker", 10);
     
     //使用与mavros通信的类读取飞控状态量
     state_from_mavros px4_state_from_mavros;
 
     // 10秒定时打印，以确保程序在正确运行
    ros::Timer timer = nh.createTimer(ros::Duration(10.0), timerCallback);   

    // 频率
    ros::Rate rate(100.0);

    //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>Main Loop<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
    while(ros::ok())
    {
        //回调一次 更新传感器状态
        ros::spinOnce();

         // 将采集定位信息及偏航角信息发送至飞控
        send_to_px4();
        //发布相机位姿到fastplanner
        send_to_planner();

         // 发布无人机状态至其他节点，如px4_controll.cpp
        pub_to_nodes(px4_state_from_mavros.px4_state);

        rate.sleep();
    }

    return 0;
}

void send_to_px4()
{  
    geometry_msgs::PoseStamped vision;
    //使用vins定位
    if(input_source == 1)
    {
        vision.pose.position.x = px4_pose_vins[0];
        vision.pose.position.y = px4_pose_vins[1];
        vision.pose.position.z = px4_pose_vins[2];

        vision.pose.orientation.x = px4_q_vins.x();
        vision.pose.orientation.y = px4_q_vins.y();
        vision.pose.orientation.z = px4_q_vins.z();
        vision.pose.orientation.w = px4_q_vins.w();

    }
    if(input_source == 2)
    {
        vision.pose.position.x = px4_pose_msckf[0];
        vision.pose.position.y = px4_pose_msckf[1];
        vision.pose.position.z = px4_pose_msckf[2];

        vision.pose.orientation.x = px4_q_msckf.x();
        vision.pose.orientation.y = px4_q_msckf.y();
        vision.pose.orientation.z = px4_q_msckf.z();
        vision.pose.orientation.w = px4_q_msckf.w();
    }

    vision.header.stamp = ros::Time::now();
    vision_mavros_pub.publish(vision);  //发布定位数据给飞控

}

void send_to_planner()
{
    geometry_msgs::PoseStamped camera_pose;
    camera_pose.header.frame_id = "world";
    //使用vins定位
    if(input_source == 1)
    {  
            camera_pose.pose.position.x = px4_camera_pose_vins[0];
            camera_pose.pose.position.y = px4_camera_pose_vins[1];
            camera_pose.pose.position.z = px4_camera_pose_vins[2];

            camera_pose.pose.orientation.x = px4_camera_q_vins.x();
            camera_pose.pose.orientation.y = px4_camera_q_vins.y();
            camera_pose.pose.orientation.z = px4_camera_q_vins.z();
            camera_pose.pose.orientation.w = px4_camera_q_vins.w();
    }
        //使用msckf定位
    if(input_source == 2)
    {  
            camera_pose.pose.position.x = px4_pose_msckf[0] + camera_pose_bias_x;
            camera_pose.pose.position.y = px4_pose_msckf[1] + camera_pose_bias_y;
            camera_pose.pose.position.z = px4_pose_msckf[2] + camera_pose_bias_z;

            Eigen::Quaterniond q_camera = px4_q_msckf * q_body_camera;
            q_camera = q_camera.normalized();

            camera_pose.pose.orientation.x = q_camera.x();
            camera_pose.pose.orientation.y = q_camera.y();
            camera_pose.pose.orientation.z = q_camera.z();
            camera_pose.pose.orientation.w = q_camera.w();
    }
    camera_pose.header.stamp = ros::Time::now();
    camera_pose_pub.publish(camera_pose);

}

void pub_to_nodes(px4_msgs::Px4State State_from_mavros)
{
    /********************************************
     *发布无人机当前状态信息至其他节点
     *********************************************/
    //为自定义的消息赋值
    Px4_State.header.stamp = ros::Time::now();
    Px4_State = State_from_mavros;
    px4_state_pub.publish(Px4_State);

    /********************************************
     *发布无人机当前odometry,用于导航及rviz显示
     *********************************************/
    nav_msgs::Odometry px4_odom;
    px4_odom.header.stamp = ros::Time::now();
    px4_odom.header.frame_id = "world";
    px4_odom.child_frame_id = "base_link";

    px4_odom.pose.pose.position.x = Px4_State.position[0];
    px4_odom.pose.pose.position.y = Px4_State.position[1];
    px4_odom.pose.pose.position.z = Px4_State.position[2];
     // 导航算法规定 高度不能小于0
    if (px4_odom.pose.pose.position.z <= 0)
    {
        px4_odom.pose.pose.position.z = 0.01;
    }

    px4_odom.pose.pose.orientation =  Px4_State.attitude_q;
    px4_odom.twist.twist.linear.x = Px4_State.velocity[0];
    px4_odom.twist.twist.linear.y = Px4_State.velocity[1];
    px4_odom.twist.twist.linear.z = Px4_State.velocity[2];
    px4_odom_pub.publish(px4_odom);

    /********************************************
     *发布无人机轨迹
     *********************************************/
    //先读取位姿
    geometry_msgs::PoseStamped px4_pose;
    px4_pose.header.stamp = ros::Time::now();
    px4_pose.header.frame_id = "world";
    px4_pose.pose.position.x = Px4_State.position[0];
    px4_pose.pose.position.y = Px4_State.position[1];
    px4_pose.pose.position.z = Px4_State.position[2];

    px4_pose.pose.orientation = Px4_State.attitude_q;
    //发布无人机的位姿 和 轨迹 用作rviz中显示
    px4_pose_history.insert(px4_pose_history.begin(), px4_pose);  //在数组第一个元素前插入无人机当前位姿(所以说前面数据的永远是最新的姿态)
    if(px4_pose_history.size() > tra_windows)
    {
        px4_pose_history.pop_back();  //若轨迹数组超出最大长度，删除最后一个位姿数据
    }

    nav_msgs::Path px4_trajectory;
    px4_trajectory.header.stamp = ros::Time::now();
    px4_trajectory.header.frame_id = "world";
    px4_trajectory.poses = px4_pose_history;
    px4_trajectory_pub.publish(px4_trajectory);

    /********************************************
    *发布无人机marker用于rviz
    *********************************************/
    visualization_msgs::Marker px4_marker;
    px4_marker.header.stamp = ros::Time::now();
    px4_marker.header.frame_id = "world";
    px4_marker.ns = "mesh";
    px4_marker.id = 0;
    px4_marker.type = 10;
    px4_marker.action = 0;

    px4_marker.pose.position.x = Px4_State.position[0];
    px4_marker.pose.position.y = Px4_State.position[1];
    px4_marker.pose.position.z = Px4_State.position[2];
    px4_marker.pose.orientation = Px4_State.attitude_q;

    px4_marker.scale.x = 1;
    px4_marker.scale.y = 1;
    px4_marker.scale.z = 1;

    px4_marker.color.r = 0;
    px4_marker.color.g = 1;
    px4_marker.color.b = 0;
    px4_marker.color.a = 1;
    //px4_marker.lifetime = 0;
   // px4_marker.frame_locked = False;
    px4_marker.mesh_resource = "file:///home/wangrui/VSLAM/src/Fast-Planner/uav_simulator/Utils/odom_visualization/meshes/hummingbird.mesh";
    //px4_marker.mesh_use_embedded_materials = False;
    px4_marker_pub.publish(px4_marker);

}
